/**
 * M5麦克纳姆轮遥控小车M5麦克纳姆轮遥控小车
 * 
 * https://www.xpstem.com/maker/m5-mecanumcar
 * author: billy zhang
 */

#ifndef __LEGO_MOTOR_H__
#define __LEGO_MOTOR_H__

#include "Arduino.h"

class LegoMotor
{
    public:
        LegoMotor(uint8_t baseAddr, uint8_t pos) 
          : baseAddr_(baseAddr), pos_(pos) {};
        void SetMode(uint8_t mode);

        int32_t GetEncoderValue();
        void SetEncoderValue(int32_t encode);
        
        void SetMotorSpeed(int8_t duty);
        int8_t GetMotorSpeed();
        
        int8_t GetMotorSpeed20MS();
        
        void SetPositionPID(uint8_t kp, uint8_t ki, uint8_t kd);
        void SetPositionPoint(int32_t position_point);
        void SetPostionPIDMaxSpeed(uint8_t max_pwm);

        void SetSpeedPID(uint8_t kp, uint8_t ki, uint8_t kd);
        void SetSpeedPoint(int8_t speed_point);

    private:
       const uint8_t baseAddr_;
       const uint8_t pos_;
};

#endif
